#include "pointcloud_tools.h"
#include "point_display.h"
#include "ai_common.h"
#include <iostream>
#include "lidar_tools.h"
#include "point_cloud.h"
#include "point_common.h"
#include <fstream>
#include <unordered_map>
#include <unordered_set>
#include <map>
#include <cassert>

using namespace  pointcloud_tools;
void preprocess_pointcloud(std::vector<Points> const& cloud, BlockConfig &bcfg, std::vector<size_t>& voxel_map){
    
    // ===============
    // voxilization & block partition
    // ===============
    // X: -8~+8 Y:-8~+8 Z:-4~+4
    // x_rsolu: 0.02(16/0.04=400) y_rsolu: 0.02 (16/0.04=400) z_rsolu:0.2 (8/0.2=40)
    // vox_size: 800x800x40
    // block_size: 8*8*4
    // max_block_num: 100*100*10
    float x_rsolu = 0.02;
    float y_rsolu = 0.02;
    float z_rsolu = 0.02;
    int8_t x_min, x_max;
    int8_t y_min, y_max;
    int8_t z_min, z_max;

    x_min = -8;
    x_max = 8;
    y_min = -2;
    y_max = 10;
    z_min = -8;
    z_max = 8;

    uint16_t vox_x_max = (uint16_t) ((x_max - x_min) / x_rsolu);
    uint16_t vox_y_max = (uint16_t) ((y_max - y_min) / y_rsolu);
    uint16_t vox_z_max = (uint16_t) ((z_max - z_min) / z_rsolu);

    // std::unordered_set<uint32_t> vox_bool;
    std::unordered_map<uint32_t, uint32_t> vox_bool;
    uint32_t  voxel_num = 0;

    for (size_t i = 0; i < cloud.size(); i++) {
        Points const& n = cloud[i];
        if (n.X <= x_min || n.X >= x_max || n.Y <= y_min || n.Y >= y_max || n.Z <= z_min || n.Z >= z_max){
            voxel_map.push_back(0);
            continue;
        }

        // Point_ALL_INT xyz_vox_tmp;
        uint16_t x,y,z;
        int8_t ch0, ch1, ch2;
        x = (uint64_t)((n.X-x_min) / x_rsolu); // 0~800
        y = (uint64_t)((n.Y-y_min) / y_rsolu); // 0~800
        z = (uint64_t)((n.Z-z_min) / z_rsolu ); // 0~400
        ch0 = (int64_t) (n.X * 16); // -8~8 *16 -> -128 ~ 128
        ch1 = (int64_t) (n.Y * 16); // -8~8 *16 -> -128 ~ 128
        ch2 = (int64_t) ((n.Z-0.7) * 32); // -4~4 *32 -> -128 ~ 128


        uint32_t hash_key = z + y * vox_z_max + x * vox_z_max * vox_y_max;

        auto iter = vox_bool.find(hash_key);
        if (iter == vox_bool.end()) {
            // new voxel points
            vox_bool[hash_key] = voxel_num;
            bcfg.pt_data.push_back((x | (y << 12) | (z <<24)));
            bcfg.in_data.push_back((ch0 | (ch1 << 8) | (ch2 << 16)));
            // vox_data.push_back(xyz_vox_tmp);
            voxel_map.push_back(voxel_num+1);
            voxel_num++;
        } else{       
            voxel_map.push_back(iter->second+1);
        }
    }
    printf("Push point vector ok, total point number %lu.\n",bcfg.in_data.size());

}

void export_INTPOINT_results(std::vector<Point_ALL_INT> int_points, std::string coo_filename, std::string data_filename){
    std::ofstream out(coo_filename);
    if (out.is_open())
    {
        for(int i=0; i < int_points.size(); i++){
            out << int_points[i].x << "," << int_points[i].y << "," << int_points[i].z << std::endl;
        }
    }
    out.close();

    std::ofstream out2(data_filename);
    if (out2.is_open())
    {
        for(int i=0; i < int_points.size(); i++){
            out2 << int_points[i].ch0 << "," << int_points[i].ch1 << "," << int_points[i].ch2 <<  "," << int_points[i].ch3 << std::endl;
        }
    }
    out2.close();

}
union union_data64to8
{
	uint64_t in64;
	int8_t out8[8];
}union_out64to8;
void function_avg(std::vector<int32_t> &point_vector, std::vector<uint64_t> const rs_data){
    for (size_t i = 0; i < rs_data.size(); i++){
        union_out64to8.in64 = rs_data[i];
        int32_t _sum = 0;
        for (int j = 0; j < 8; j++){
            _sum += union_out64to8.out8[j];
        }
        point_vector.push_back(_sum);
    }
}

float calculateAverage(const std::vector<int32_t>& points_vector) {
    if (points_vector.empty()) {
        return 0.0; // 处理空向量的情况，避免除零错误
    }

    int64_t sum = 0; // 使用 int64_t 避免整数溢出
    for (int32_t value : points_vector) {
        sum += value;
    }

    return static_cast<double>(sum) / points_vector.size();
}

int main(int argc, char** argv) {
    // Offline mode
    std::string lidar_ip = "192.168.10.108";
    int ret = 0;
    //Step 1 : Specify a pcap file, which contain the lidar's pointcloud packet.
    std::string pcap_filename = "../data/lidar_sample/two_chair_video.pcap"; 
    //Specify a calibration file, which contain the lidar's calibration data.
    std::string cal_filename = "../data/lidar_sample//device.cal";
    //Step 2 : Specify the pcap file, calibtation file to play.
    //The ip address and udp destination port is used to filter the pcap file to play the special lidar data. 
    zvision::OfflinePointCloudProducer player(pcap_filename, cal_filename, lidar_ip, 2368);
    int size = 0;
    zvision::DeviceType type = zvision::LidarUnknown;
    if (ret = player.GetPointCloudInfo(size, type)){
        printf("OfflinePointCloudProducer GetPointCloudInfo failed, ret = %d.\n", ret);
        return 0;
    } else {
        if (0 == size){
            printf("No pointcloud found for lidar %s:%d.\n", lidar_ip.c_str(), 2368);
            return 0;
        } else {
            printf("Sample offline pointclouds, size = %d.\n", size);
        }
    }
    PointDisplay display;
    //Step 4: Iterate the pointcloud.
    std::vector<Points> points;
    std::vector<Point_ALL_INT> int_points;
    std::vector<size_t> voxel_map;
    std::vector<int32_t> points_vector;
    /*  ------------
        Example Code Start
        -----------  */
    Pointcloud_utils ptu;
    ptu.layer_config.cfg_inch_loop_num = 0;
    ptu.layer_config.cfg_core_enable = 1; // Only 8 output channels
    ptu.layer_config.cfg_shift_num = 8; 
    ptu.DDR_BASE_ADDR = 0x80000000;
    ptu.layer_config.BASE_WT_ADDR = ptu.DDR_BASE_ADDR + 0x000C00000; // 0x650000400; 

    //Simulation Model
    ptu.ddrSize = 8 * 8 * 1024 * 1024 * sizeof(int); //256MB
    assert(ptu.Model_DDR_alloc("DDR_32MB.bin"));
    // Data Init
    size_t dlen = readBinToVec(ptu.layer_config.wt_data, "../data/second_sample/weight/weight_layer1.bin");
    assert(dlen > 0);
    /*  ------------
        Example Code End
        -----------  */

    for (int i = 0; i < size; ++i) {
    // for (int i = 0; i < size; ++i) {
        zvision::PointCloud pointcloud;
        if (ret = player.GetPointCloud(i, pointcloud)){
            printf("GetPointCloud error, frame number is %d, ret = %d.", i, ret);
            return 0;
        }
        points.clear();
        for (auto const& p : pointcloud.points) {
            if ((p.x ==0 && p.y ==0 && p.z == 0) || p.reflectivity == 0)
                continue;
            // printf("I:%d \n", p.reflectivity);
            points.push_back(Points {p.x, p.y, p.z, p.reflectivity});
        }

        // Step 5: Preprocess for voxel points
        voxel_map.clear();
        int_points.clear();
        points_vector.clear();

        BlockConfig bcfg; 
        bcfg.pt_data.clear();
        bcfg.in_data.clear();

        preprocess_pointcloud(points, bcfg, voxel_map);
        // export_INTPOINT_results(int_points, "./coo.txt", "./in_feature.txt");

        // // Block Config 
        // bcfg.BASE_PT_ADDR = ptu.DDR_BASE_ADDR;
        // bcfg.BASE_IN_ADDR = ptu.DDR_BASE_ADDR + 0x000400000;
        // bcfg.BASE_RS_ADDR = ptu.DDR_BASE_ADDR + 0x000800000;
        // bcfg.cfg_cam_valid_num = uint16_t(bcfg.pt_data.size());
        // bcfg.cfg_total_point_num = uint16_t(bcfg.pt_data.size());

        // ptu.block_config.push_back(bcfg);
        // ptu.DDR_init();
        // ptu.Model_SubConvCompute();
        // function_avg(points_vector, ptu.block_config[0].rs_data);
        // float avg = calculateAverage(points_vector);
        // printf("avg of points_vector: %f \n", avg);
        // assert(voxel_map.size() == points.size());
        // for (size_t j = 0; j < points.size(); j++) {
        //     // printf("%d %d\n", i, voxel_map[i]);
        //     // if (points_vector[i].is_classified_as_object())
        //     if(voxel_map[j] == 0) points[j].kind = 0;
        //     else {
        //         if (points_vector[voxel_map[j]-1] > -8){
        //             points[j].kind = 1;
        //             // printf("Mark \n");
        //         }
        //     }
        // }
        

        display.updatePointCloud(points);
        display.display();

        if (display.wasStopped()) {
            break;
        }

        // std::this_thread::sleep_for(std::chrono::milliseconds(100));
    }

    return 0;

    // PointDisplay display;
}

  



